#include <kalman.h>
#include <cmath>
#include <ros/ros.h>
#include <kalman_joint/KalmanJoint.h>
#include <cv_joint_angle/Forearm.h>
#include <cv_joint_angle/Bucket.h>
#include <mavros_msgs/ExcavatorInclination.h>

#define deg_TCA 47.
#define deg_BCF 28.
#define mm_AC 68.
#define mm_BC 161.
#define deg_DFC 33.
#define deg_QFG 94.
#define deg_GFE 51.
#define mm_DF 236.8
#define mm_EF 63.6
#define deg_NQF 5.5
#define deg_KQV 108.
#define mm_QN 32.5
#define mm_QK 43.
#define mm_MN 54.
#define mm_MK 54.
#define deg_GNF 20.5
#define deg_FNQ 173.
#define mm_GN 178.
#define deg_GNF 20.5
#define mm_QV 158.
#define mm_FQ 198.
#define mm_CF 407.
#define mm_JC 47.6
#define mm_JL 184.
